#ifndef FEATURE_MANAGER_H
#define FEATURE_MANAGER_H

#include <list>
#include <algorithm>
#include <vector>
#include <numeric>
using namespace std;

#include <eigen3/Eigen/Dense>
using namespace Eigen;

#include <ros/console.h>
#include <ros/assert.h>

#include "parameters.h"

/**
 * @class FeaturePerFrame
 * @brief 一个特征点的信息
 */
class FeaturePerFrame
{
public:
  //_point:[x,y,z,u,v,vx,vy]
  FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td)
  {
    point.x() = _point(0);
    point.y() = _point(1);
    point.z() = _point(2);
    uv.x() = _point(3);
    uv.y() = _point(4);
    velocity.x() = _point(5);
    velocity.y() = _point(6);
    cur_td = td;
  }
  double cur_td;     // 所属img的td
  Vector3d point;    // 归一化平面
  Vector2d uv;       // 像素平面
  Vector2d velocity; // 速度
  double z;
  bool is_used;    // 已被使用
  double parallax; // 视差
  MatrixXd A;
  VectorXd b;
  double dep_gradient;
};

/**
 * @class FeaturePerId
 * @brief 某feature_id下的所有特征点的信息
 */
class FeaturePerId
{
public:
  const int feature_id;                      // feature的ID
  int start_frame;                           // feature的起始帧
  vector<FeaturePerFrame> feature_per_frame; // 该帧下所有的特征点信息

  int used_num;           // 使用的特征点个数
  bool is_outlier;        // 是外点
  bool is_margin;         // 已经边缘化
  double estimated_depth; // 逆深度估计
  int solve_flag;         // 求解标志位, 0 haven't solve yet; 1 solve succ; 2 solve fail;

  Vector3d gt_p;

  FeaturePerId(int _feature_id, int _start_frame)
      : feature_id(_feature_id), start_frame(_start_frame),
        used_num(0), estimated_depth(-1.0), solve_flag(0)
  {
  }

  int endFrame();
};

/**
 * @class FeatureManager
 * @brief 用于管理FeaturePerId
 */
class FeatureManager
{
public:
  FeatureManager(Matrix3d _Rs[]);

  void setRic(Matrix3d _ric[]);

  void clearState();

  int getFeatureCount();

  bool addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td);
  void debugShow();
  vector<pair<Vector3d, Vector3d>> getCorresponding(int frame_count_l, int frame_count_r);

  void setDepth(const VectorXd &x);
  void removeFailures();
  void clearDepth(const VectorXd &x);
  VectorXd getDepthVector();
  void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]);
  void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P);
  void removeBack();
  void removeFront(int frame_count);
  void removeOutlier();

  list<FeaturePerId> feature; // 滑动窗口下所有的角点信息
  int last_track_num;         // 特征点之前被跟踪的次数

private:
  double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count);
  const Matrix3d *Rs;       // 当前帧->第一帧旋转
  Matrix3d ric[NUM_OF_CAM]; // 相机->imu旋转
};

#endif